#include "torque_control_queue.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

int TorqueControlQueIsFull(const TorqueControlQue* torque_queue)
{
    if ((torque_queue->rear + 1) % MAX_TORQUE_CONTROL_QUE_LEN == torque_queue->front)
        return 1;
    return 0;
}

int TorqueControlQueReset(TorqueControlQue* torque_queue)
{
    torque_queue->rear = -1;
    torque_queue->front = -1;
    memset(torque_queue->torque_que, 0, MAX_TORQUE_CONTROL_QUE_LEN * sizeof(TorqueControlVal));
    return 0;
}

int TorqueControlQueIsEmpty(TorqueControlQue* torque_queue)
{
    if (torque_queue->front == -1)
        return 1;
    return 0;
}

short TorqueControlEnqueue(TorqueControlQue* torque_queue, const TorqueControlVal pos)
{
    if (TorqueControlQueIsFull(torque_queue))
    {
        return -1;
    }
    else
    {
        if (torque_queue->front == -1)
            torque_queue->front = 0;
        torque_queue->rear = (torque_queue->rear + 1) % MAX_TORQUE_CONTROL_QUE_LEN;
        torque_queue->torque_que[torque_queue->rear] = pos;
    }
    return ((torque_queue->rear + MAX_TORQUE_CONTROL_QUE_LEN - torque_queue->front) % MAX_TORQUE_CONTROL_QUE_LEN) + 1;
}

short TorqueControlDequeue(TorqueControlQue* torque_queue, TorqueControlVal* pos)
{
    if (pos == NULL)
        return -1;
    if (TorqueControlQueIsEmpty(torque_queue))
    {
        return -1;
    }
    else
    {
        *pos = torque_queue->torque_que[torque_queue->front];
        if (torque_queue->front == torque_queue->rear)
        {
            torque_queue->front = -1;
            torque_queue->rear = -1;
        }
        else
        {
            torque_queue->front = (torque_queue->front + 1) % MAX_TORQUE_CONTROL_QUE_LEN;
        }
        return 0;
    }
}

int TorqueControlQueSize(const TorqueControlQue* torque_queue)
{
    if (torque_queue->front == -1)
    {
        return 0;
    }
    int size = ((torque_queue->rear + MAX_TORQUE_CONTROL_QUE_LEN - torque_queue->front) % MAX_TORQUE_CONTROL_QUE_LEN);
    return size + 1;
}
